#include <robot/robot.h>
#include <robot/kuka/carmclient.h>
#include <buola/app/ccmdline.h>

using namespace buola;
using namespace buola::robot;

int main(int pNArgs,char **pArgs)
{
    buola_init(pNArgs,pArgs);

    std::vector<double> jpos{-1.57,-1.57,1.57,0.00,0.78,0.00};
    kuka::CArmPose lPose(jpos);
    mat::CVec3d lPos;
    mat::CQuat_d lRot;
    forward_kinematics(lPose,lPos,lRot);
    msg_info() << lPos << mat::C3DRotation(lRot) << "\n";
    abort();
    
    
    try
    {
        kuka::CArmClient lClient;

        if(cmd_line().ArgCount())
        {
            lClient.Init(blackbox::CClientProxy(cmd_line().Arg(0))["arm"]);
        }
        else
        {
            lClient.Init();
        }

        while(true)
        {
            mat::CVec3d lGoalP;
            mat::C3DRotation lGoalR;

            msg_info() << "enter character 'p' followed by x y z or 'o' followed by y p r\n";

            char lChar;

            do
            {
                gIn >> lChar;
            } while(lChar!='p'&&lChar!='o'&&lChar!='h'&&lChar!='i'&&lChar!='j'
                  &&lChar!='q'&&lChar!='s'&&lChar!='z'&&lChar!='c'&&lChar!='x');

            if(lChar=='p')
            {
                gIn >> lGoalP.x() >> lGoalP.y() >> lGoalP.z();

                lClient.SetGoal(lGoalP);
            }
            else if(lChar=='c')
            {
                gIn >> lGoalP.x() >> lGoalP.y() >> lGoalP.z();

                lClient.SetGoal(lGoalP,true);
            }
            else if(lChar=='h')
            {
                lClient.SetGoal(mat::CVec3d(0,700,600),mat::C3DRotation(50,0,180));
            }
            else if(lChar=='i')
            {
                mat::CVec3d lPos;
                mat::C3DRotation lRot;
                std::vector<double> lJoints;

                lClient.GetPose(lPos,lRot);
                lClient.GetPose(lJoints);

                msg_info() << "pos=" << lPos << "\n";
                msg_info() << "rot=" << lRot*180.0/M_PI << "\n";
                msg_info() << "joints=";
                for(int i=0;i<6;i++)
                {
                    if(i) msg_info() << ',';
                    msg_info() << lJoints[i];
                }
                msg_info() << "\n";

                kuka::CArmPose lPose(lJoints.data());

                mat::CVec3d lAP;
                mat::CQuat_d lAR;

                forward_kinematics(lPose,lAP,lAR);

                msg_info() << "forward kinematics pos " << lAP << "\n";
                msg_info() << "forward kinematics rot " << mat::C3DRotation(-lAR)*180.0/M_PI << "\n";

                std::vector<kuka::CArmPose> lPoses;
                inverse_kinematics(lAP,lAR,lPoses);
            }
            else if(lChar=='j')
            {
                int lJ;
                double lValue;
                gIn >> lJ >> lValue;
                lClient.SetGoal(lJ,deg2rad(lValue));
            }
            else if(lChar=='s')
            {
                double lLinear;
                double lRot;
                gIn >> lLinear >> lRot;
                lClient.SetMaxSpeed(lLinear,deg2rad(lRot));
            }
            else if(lChar=='o')
            {
                gIn >> lGoalR.a >> lGoalR.b >> lGoalR.c;

                lClient.SetGoal(lGoalR*M_PI/180.0);
            }
            else if(lChar=='q')
            {
                break;
            }
            else if(lChar=='z')
            {
                lClient.Stop();
            }
            else if(lChar=='x')
            {
                lClient.DoCorrection(mat::CVec3d(1,0,0));
                sleep(5);
                lClient.Stop();
            }            
            else
            {
                msg_info() << "unknown option\n";
            }
        }
    }
    catch(std::exception &pE)
    {
        msg_error() << "caught exception:" << pE.what() << "\n";
    }

    return buola_finish();
}
